#pragma once
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <array>
#include <vector>

// 设置超参数：极坐标网格的通道数与径向bins数，根据你的LIDAR传感器和需求调整
constexpr int numChannel = 180;    // 例如180个角度bin，2度一格
constexpr int numBin = 100;        // 例如100个半径区间
constexpr float rMin = 3.4f;
constexpr float rMax = 120.0f;

// 地面高度不存在的标记高度
constexpr float tHmin = -3.0f;     // 可自定义你项目中的最小高度

// 极坐标网格中的单元格结构
struct Cell {
    float minZ;         // 当前单元格内的最小z值，用于判地面
    bool isGround;      // 是否被判定为地面
    float hGround;      // 地面高度（可以冗余保存）

    Cell() : minZ(999.0f), isGround(false), hGround(tHmin) {}
    void updateMinZ(float z) { if (z < minZ) minZ = z; }
    float getHeight() const { return minZ; }
    void updateHeight(float val) { minZ = val; }
    void updateGround() { isGround = true; hGround = minZ; }
    bool isThisGround() const { return isGround; }
};

namespace ground_remove {

/**
 * @brief 点云地面与高点分离
 * @param[in]  cloud           输入点云（pcl::PointCloud<PointXYZI>）
 * @param[out] groundCloud     输出地面点
 * @param[out] elevatedCloud   输出高点（非地面点）
 */
void removeGround(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud,
                  pcl::PointCloud<pcl::PointXYZI>::Ptr& groundCloud,
                  pcl::PointCloud<pcl::PointXYZI>::Ptr& elevatedCloud);

// 用于xy坐标到极坐标网格cell索引转换的函数
void getCellIndexFromPoints(float x, float y, int& chIdx, int& binIdx);

} // namespace ground_remove